<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">
<html><head><meta http-equiv="Content-Type" content="text/html;charset=utf-8">
<title>par_trajectory_planning: /home/wouter/ros_packages/thp-pj2-08/par_trajectory_planning/src/steppermotor.cpp Source File</title>
<link href="doxygen.css" rel="stylesheet" type="text/css">
<link href="tabs.css" rel="stylesheet" type="text/css">
</head><body>

<table border="0">
<tr><td valign="top" style="border-right: 1px solid #333; padding-right: 10px;" >
<p>
Project: <a href="http://ros.org/wiki/par_trajectory_planning">par_trajectory_planning</a>
</p>
<p>
License: BSD
</p>
<p>
Dependencies: 
<ul>
  <li>std_msgs</li>
  <li>roscpp</li>
</ul>
</p>
<p>
Used by:
None<br />
</p>
<p>
<a href="../../index.html">All Packages</a>
</p>

</div>
</td><td valign="top" style="padding-left: 10px;" >
<!-- Generated by Doxygen 1.7.1 -->
<div class="navigation" id="top">
  <div class="tabs">
    <ul class="tablist">
      <li><a href="index.html"><span>Main&nbsp;Page</span></a></li>
      <li><a href="namespaces.html"><span>Namespaces</span></a></li>
      <li><a href="annotated.html"><span>Classes</span></a></li>
      <li class="current"><a href="files.html"><span>Files</span></a></li>
    </ul>
  </div>
  <div class="tabs2">
    <ul class="tablist">
      <li><a href="files.html"><span>File&nbsp;List</span></a></li>
      <li><a href="globals.html"><span>File&nbsp;Members</span></a></li>
    </ul>
  </div>
<div class="header">
  <div class="headertitle">
<h1>/home/wouter/ros_packages/thp-pj2-08/par_trajectory_planning/src/steppermotor.cpp</h1>  </div>
</div>
<div class="contents">
<a href="steppermotor_8cpp.html">Go to the documentation of this file.</a><div class="fragment"><pre class="fragment"><a name="l00001"></a>00001 
<a name="l00005"></a>00005 <span class="preprocessor">#include &lt;<a class="code" href="steppermotor_8h.html">par_trajectory_planning/steppermotor.h</a>&gt;</span>
<a name="l00006"></a>00006 <span class="preprocessor">#include &lt;<a class="code" href="config_8h.html">par_trajectory_planning/config.h</a>&gt;</span>
<a name="l00007"></a>00007 <span class="preprocessor">#include &lt;cstdio&gt;</span>
<a name="l00008"></a>00008 <span class="preprocessor">#include &lt;cmath&gt;</span>
<a name="l00009"></a>00009 <span class="preprocessor">#include &lt;cerrno&gt;</span>
<a name="l00010"></a>00010 
<a name="l00011"></a><a class="code" href="classStepperMotor.html#aeb9715654c67df9981f2690bf332f897">00011</a> <span class="keywordtype">void</span> <a class="code" href="classStepperMotor.html#aeb9715654c67df9981f2690bf332f897" title="Function for initializing the motor.">StepperMotor::init</a>()
<a name="l00012"></a>00012 {
<a name="l00013"></a>00013     std::cout &lt;&lt; <span class="stringliteral">&quot;StepperMotor::init()&quot;</span> &lt;&lt; std::endl;
<a name="l00014"></a>00014     <a class="code" href="classStepperMotor.html#af97e94b64dccbdd2bfea15113aea06dc">motions</a> = 0;
<a name="l00015"></a>00015 }
<a name="l00016"></a>00016 
<a name="l00017"></a><a class="code" href="classStepperMotor.html#a4b25a3c298a270d4952f13e25bfb8acc">00017</a> <span class="keywordtype">void</span> <a class="code" href="classStepperMotor.html#a4b25a3c298a270d4952f13e25bfb8acc" title="Function for starting the motor.">StepperMotor::start</a>()
<a name="l00018"></a>00018 {
<a name="l00019"></a>00019     std::cout &lt;&lt; <span class="stringliteral">&quot;StepperMotor::start()&quot;</span> &lt;&lt; std::endl;
<a name="l00020"></a>00020     uint16_t stat_ax01[2], stat_ax02[2], stat_ax03[3];
<a name="l00021"></a>00021     <span class="keywordtype">int</span> i, n, j;
<a name="l00022"></a>00022     
<a name="l00023"></a>00023     j = 0;
<a name="l00024"></a>00024     std::cout &lt;&lt; <span class="stringliteral">&quot;REPEAT MOTIONS: &quot;</span> &lt;&lt; <a class="code" href="classStepperMotor.html#ab12d157b9b402915289ce35a2c821141">repeat_motions</a> &lt;&lt; std::endl;
<a name="l00025"></a>00025     std::cout &lt;&lt; <span class="stringliteral">&quot;MOTIONS: &quot;</span> &lt;&lt; <a class="code" href="classStepperMotor.html#af97e94b64dccbdd2bfea15113aea06dc">motions</a> &lt;&lt; std::endl;
<a name="l00026"></a>00026 
<a name="l00027"></a>00027     <span class="keywordflow">while</span>(j &lt; <a class="code" href="classStepperMotor.html#ab12d157b9b402915289ce35a2c821141">repeat_motions</a> &amp;&amp; !<a class="code" href="classStepperMotor.html#aeaa6702b2fd2d66bf10dd32999f70d8a">stop_motion</a>)
<a name="l00028"></a>00028     {
<a name="l00029"></a>00029         <span class="keywordflow">for</span>(i=1; i&lt;=<a class="code" href="classStepperMotor.html#af97e94b64dccbdd2bfea15113aea06dc">motions</a> &amp;&amp; !<a class="code" href="classStepperMotor.html#aeaa6702b2fd2d66bf10dd32999f70d8a">stop_motion</a>; i++)
<a name="l00030"></a>00030         {
<a name="l00031"></a>00031                 modbus_set_slave(<a class="code" href="classStepperMotor.html#a1f622627c7b054721a3313389cb48ad8">ctx</a>, 0);
<a name="l00032"></a>00032             n = modbus_write_register(<a class="code" href="classStepperMotor.html#a1f622627c7b054721a3313389cb48ad8">ctx</a>, 0x001E, 0x2000);
<a name="l00033"></a>00033                 <span class="comment">//printf(&quot;errno: %s\n&quot;, modbus_strerror(errno));</span>
<a name="l00034"></a>00034                     <span class="comment">//std::cout &lt;&lt; &quot;--&gt; [1] write result: &quot; &lt;&lt; n &lt;&lt; std::endl;</span>
<a name="l00035"></a>00035                 usleep(<a class="code" href="config_8h.html#a5eeba5b71a3abb83274b07913815b1ca">MODBUS_MAX_BCAST_TIME</a>);
<a name="l00036"></a>00036                 <span class="comment">// turn start input on</span>
<a name="l00037"></a>00037                 n = modbus_write_register(<a class="code" href="classStepperMotor.html#a1f622627c7b054721a3313389cb48ad8">ctx</a>, 0x001E, 0x2100 <span class="comment">/*+ i*/</span> );
<a name="l00038"></a>00038                 <span class="comment">//printf(&quot;errno: %s\n&quot;, modbus_strerror(errno));</span>
<a name="l00039"></a>00039                     <span class="comment">//std::cout &lt;&lt; &quot;--&gt; [2] write result: &quot; &lt;&lt; n &lt;&lt; std::endl;</span>
<a name="l00040"></a>00040                 usleep(<a class="code" href="config_8h.html#a5eeba5b71a3abb83274b07913815b1ca">MODBUS_MAX_BCAST_TIME</a>);
<a name="l00041"></a>00041                 <span class="comment">// turn start input off</span>
<a name="l00042"></a>00042                 n = modbus_write_register(<a class="code" href="classStepperMotor.html#a1f622627c7b054721a3313389cb48ad8">ctx</a>, 0x001E, 0x2000 <span class="comment">/*+ i*/</span> );
<a name="l00043"></a>00043                     <span class="comment">//std::cout &lt;&lt; &quot;--&gt; [3] write result: &quot; &lt;&lt; n &lt;&lt; std::endl;</span>
<a name="l00044"></a>00044                 <span class="comment">//printf(&quot;errno: %s\n&quot;, modbus_strerror(errno));    </span>
<a name="l00045"></a>00045                 usleep(<a class="code" href="config_8h.html#a5eeba5b71a3abb83274b07913815b1ca">MODBUS_MAX_BCAST_TIME</a>);
<a name="l00046"></a>00046         
<a name="l00047"></a>00047 
<a name="l00048"></a>00048                 stat_ax01[0] = stat_ax02[0] = stat_ax03[0] = 0x0000;
<a name="l00049"></a>00049                 stat_ax01[1] = stat_ax02[1] = stat_ax03[1] = 0x0000;
<a name="l00050"></a>00050                 <span class="keywordflow">while</span>( (stat_ax01[0] &amp; 0x2000) == 0 ||
<a name="l00051"></a>00051                        (stat_ax02[0] &amp;&amp; 0x2000) == 0 ||
<a name="l00052"></a>00052                        (stat_ax03[0] &amp;&amp; 0x2000) == 0 )
<a name="l00053"></a>00053                 {
<a name="l00054"></a>00054                         modbus_set_slave(<a class="code" href="classStepperMotor.html#a1f622627c7b054721a3313389cb48ad8">ctx</a>, <a class="code" href="config_8h.html#ac005ae1021923c6a1e42b0040b1870d6">MODBUS_SLAVE_ADDR_01</a>);
<a name="l00055"></a>00055                         n = modbus_read_registers(<a class="code" href="classStepperMotor.html#a1f622627c7b054721a3313389cb48ad8">ctx</a>, 0x0020, 2, stat_ax01); 
<a name="l00056"></a>00056                         usleep(<a class="code" href="config_8h.html#a2055321b80999b0cd236f4f934a9dd9c">MODBUS_MAX_PROC_TIME</a>);
<a name="l00057"></a>00057 
<a name="l00058"></a>00058                         modbus_set_slave(<a class="code" href="classStepperMotor.html#a1f622627c7b054721a3313389cb48ad8">ctx</a>, <a class="code" href="config_8h.html#a24562b230331cfd7c8661641bd339c95">MODBUS_SLAVE_ADDR_02</a>);
<a name="l00059"></a>00059                         n = modbus_read_registers(<a class="code" href="classStepperMotor.html#a1f622627c7b054721a3313389cb48ad8">ctx</a>, 0x0020, 2, stat_ax02); 
<a name="l00060"></a>00060                         usleep(<a class="code" href="config_8h.html#a2055321b80999b0cd236f4f934a9dd9c">MODBUS_MAX_PROC_TIME</a>);
<a name="l00061"></a>00061 
<a name="l00062"></a>00062                         modbus_set_slave(<a class="code" href="classStepperMotor.html#a1f622627c7b054721a3313389cb48ad8">ctx</a>, <a class="code" href="config_8h.html#ae58cb2aa8d03cd0343cf58341571128f">MODBUS_SLAVE_ADDR_03</a>);
<a name="l00063"></a>00063                         n = modbus_read_registers(<a class="code" href="classStepperMotor.html#a1f622627c7b054721a3313389cb48ad8">ctx</a>, 0x0020, 2, stat_ax03); 
<a name="l00064"></a>00064                         usleep(<a class="code" href="config_8h.html#a2055321b80999b0cd236f4f934a9dd9c">MODBUS_MAX_PROC_TIME</a>);
<a name="l00065"></a>00065                 }
<a name="l00066"></a>00066                 <span class="comment">//std::cout &lt;&lt; &quot;motor can receive new command\n&quot; &lt;&lt; std::endl;</span>
<a name="l00067"></a>00067         }  
<a name="l00068"></a>00068         j++;
<a name="l00069"></a>00069     }
<a name="l00070"></a>00070     <a class="code" href="classStepperMotor.html#aeaa6702b2fd2d66bf10dd32999f70d8a">stop_motion</a> = <span class="keyword">false</span>;
<a name="l00071"></a>00071 }
<a name="l00072"></a>00072 
<a name="l00073"></a><a class="code" href="classStepperMotor.html#af0d3c2e75ec64175fefaeeb0914432ff">00073</a> <span class="keywordtype">void</span> <a class="code" href="classStepperMotor.html#af0d3c2e75ec64175fefaeeb0914432ff" title="Function for stopping the motor.">StepperMotor::stop</a>()
<a name="l00074"></a>00074 {
<a name="l00075"></a>00075     <a class="code" href="classStepperMotor.html#aeaa6702b2fd2d66bf10dd32999f70d8a">stop_motion</a> = <span class="keyword">true</span>;
<a name="l00076"></a>00076 }
<a name="l00077"></a>00077 
<a name="l00078"></a><a class="code" href="classStepperMotor.html#ab0bd4fe04741af8b74eaecbce0f1b132">00078</a> <span class="keywordtype">void</span> <a class="code" href="classStepperMotor.html#ab0bd4fe04741af8b74eaecbce0f1b132" title="Function for initializing communication.">StepperMotor::initCom</a>()
<a name="l00079"></a>00079 {
<a name="l00080"></a>00080         <span class="keywordflow">if</span> (<a class="code" href="classStepperMotor.html#a1f622627c7b054721a3313389cb48ad8">ctx</a> == NULL)
<a name="l00081"></a>00081         {
<a name="l00082"></a>00082                 std::cout &lt;&lt; <span class="stringliteral">&quot;StepperMotor::init(): unable to create the libmodbus context.&quot;</span> &lt;&lt; std::endl;
<a name="l00083"></a>00083         }
<a name="l00084"></a>00084         
<a name="l00085"></a>00085         <span class="comment">//modbus_set_debug(ctx, 1);</span>
<a name="l00086"></a>00086         <span class="keyword">struct </span>timeval timeout_end;
<a name="l00087"></a>00087         <span class="keyword">struct </span>timeval timeout_begin;
<a name="l00088"></a>00088         modbus_get_timeout_end(<a class="code" href="classStepperMotor.html#a1f622627c7b054721a3313389cb48ad8">ctx</a>, &amp;timeout_end);
<a name="l00089"></a>00089         <span class="comment">//printf(&quot;timeout.tv_sec: %ld\n&quot;, timeout_end.tv_sec);</span>
<a name="l00090"></a>00090         <span class="comment">//printf(&quot;timeout.tv_usec: %ld\n&quot;, timeout_end.tv_usec);</span>
<a name="l00091"></a>00091         timeout_end.tv_usec = <a class="code" href="config_8h.html#a3df61e3eabec943575590ea9afa7ba89">MODBUS_TIMEOUT_END</a>;
<a name="l00092"></a>00092         modbus_set_timeout_end(<a class="code" href="classStepperMotor.html#a1f622627c7b054721a3313389cb48ad8">ctx</a>, &amp;timeout_end);
<a name="l00093"></a>00093 
<a name="l00094"></a>00094         modbus_get_timeout_begin(<a class="code" href="classStepperMotor.html#a1f622627c7b054721a3313389cb48ad8">ctx</a>, &amp;timeout_begin);
<a name="l00095"></a>00095         <span class="comment">//printf(&quot;timeout.tv_sec: %ld\n&quot;, timeout_begin.tv_sec);</span>
<a name="l00096"></a>00096         <span class="comment">//printf(&quot;timeout.tv_usec: %ld\n&quot;, timeout_begin.tv_usec);</span>
<a name="l00097"></a>00097         timeout_begin.tv_usec = <a class="code" href="config_8h.html#a0e7d142f439ef16108ba22027eb48357">MODBUS_TIMEOUT_BEGIN</a>;
<a name="l00098"></a>00098         modbus_set_timeout_begin(<a class="code" href="classStepperMotor.html#a1f622627c7b054721a3313389cb48ad8">ctx</a>, &amp;timeout_begin);
<a name="l00099"></a>00099 
<a name="l00100"></a>00100         <span class="keywordflow">if</span> ( modbus_connect(<a class="code" href="classStepperMotor.html#a1f622627c7b054721a3313389cb48ad8">ctx</a>) == -1)
<a name="l00101"></a>00101         {
<a name="l00102"></a>00102                 std::cout &lt;&lt; <span class="stringliteral">&quot;StepperMotor::init(): connection failed.&quot;</span> &lt;&lt; std::endl;
<a name="l00103"></a>00103         }
<a name="l00104"></a>00104 }
<a name="l00105"></a>00105 
<a name="l00106"></a><a class="code" href="classStepperMotor.html#a84203d36566a8ce31c35a2655d5dca99">00106</a> <span class="keywordtype">void</span> <a class="code" href="classStepperMotor.html#a84203d36566a8ce31c35a2655d5dca99" title="Function for configuring single motions.">StepperMotor::confSingleMotion</a>(<span class="keyword">const</span> <a class="code" href="structpar__trajectory__planning_1_1commands__.html">par_trajectory_planning::commands</a>&amp; cmd)
<a name="l00107"></a>00107 {
<a name="l00108"></a>00108     <a class="code" href="classStepperMotor.html#af97e94b64dccbdd2bfea15113aea06dc">motions</a> = cmd.<a class="code" href="structpar__trajectory__planning_1_1commands__.html#a01e71e9c913c7bc8ec46ac81580f1f08">abs_pos</a>.size() / 6; 
<a name="l00109"></a>00109     <a class="code" href="classStepperMotor.html#ab12d157b9b402915289ce35a2c821141">repeat_motions</a> = 1;
<a name="l00110"></a>00110 
<a name="l00111"></a>00111     <span class="keywordtype">int</span> i;
<a name="l00112"></a>00112     <span class="keywordflow">for</span>(i=0; i&lt;<a class="code" href="classStepperMotor.html#af97e94b64dccbdd2bfea15113aea06dc">motions</a>; i++)
<a name="l00113"></a>00113     {
<a name="l00114"></a>00114         <a class="code" href="classStepperMotor.html#afe3b8a4013499b95fe9b2f96be809cfe">initSingleMotion</a>(<a class="code" href="config_8h.html#ac005ae1021923c6a1e42b0040b1870d6">MODBUS_SLAVE_ADDR_01</a>, cmd.<a class="code" href="structpar__trajectory__planning_1_1commands__.html#a01e71e9c913c7bc8ec46ac81580f1f08">abs_pos</a>[ 0 + (i * 6) ], cmd.<a class="code" href="structpar__trajectory__planning_1_1commands__.html#a01e71e9c913c7bc8ec46ac81580f1f08">abs_pos</a>[ 1 + (i * 6) ], 
<a name="l00115"></a>00115             <a class="code" href="config_8h.html#a1f042e592f30e895f0e208638bc3c782">MOTOR_ACC_UP</a>, <a class="code" href="config_8h.html#aa7d1af2ddbd04215bc91d5920e3a9d25">MOTOR_ACC_LO</a>, <a class="code" href="config_8h.html#a6d01bebd179d01c2d89014e7193edfb4">MOTOR_DEC_UP</a>, <a class="code" href="config_8h.html#a559e71a5ab59fc849998a0b3bb2b4cc5">MOTOR_DEC_LO</a>, 
<a name="l00116"></a>00116             <a class="code" href="config_8h.html#ad8a5d0e0b619e6dc15bd84dd338f805c">MOTOR_OP_SPEED_UP</a>, <a class="code" href="config_8h.html#ab95f25fc2b325b0febe3fcb46d5c6ac1">MOTOR_OP_SPEED_LO</a>, <a class="code" href="config_8h.html#ad4984464598237a917da7f4d07a9fae3">MOTOR_OPM_SINGLE</a>, 0x00, i + 1); <span class="comment">// X</span>
<a name="l00117"></a>00117 
<a name="l00118"></a>00118         <a class="code" href="classStepperMotor.html#afe3b8a4013499b95fe9b2f96be809cfe">initSingleMotion</a>(<a class="code" href="config_8h.html#a24562b230331cfd7c8661641bd339c95">MODBUS_SLAVE_ADDR_02</a>, cmd.<a class="code" href="structpar__trajectory__planning_1_1commands__.html#a01e71e9c913c7bc8ec46ac81580f1f08">abs_pos</a>[ 2 + (i * 6) ], cmd.<a class="code" href="structpar__trajectory__planning_1_1commands__.html#a01e71e9c913c7bc8ec46ac81580f1f08">abs_pos</a>[ 3 + (i * 6) ],
<a name="l00119"></a>00119             <a class="code" href="config_8h.html#a1f042e592f30e895f0e208638bc3c782">MOTOR_ACC_UP</a>, <a class="code" href="config_8h.html#aa7d1af2ddbd04215bc91d5920e3a9d25">MOTOR_ACC_LO</a>, <a class="code" href="config_8h.html#a6d01bebd179d01c2d89014e7193edfb4">MOTOR_DEC_UP</a>, <a class="code" href="config_8h.html#a559e71a5ab59fc849998a0b3bb2b4cc5">MOTOR_DEC_LO</a>, 
<a name="l00120"></a>00120             <a class="code" href="config_8h.html#ad8a5d0e0b619e6dc15bd84dd338f805c">MOTOR_OP_SPEED_UP</a>, <a class="code" href="config_8h.html#ab95f25fc2b325b0febe3fcb46d5c6ac1">MOTOR_OP_SPEED_LO</a>, <a class="code" href="config_8h.html#ad4984464598237a917da7f4d07a9fae3">MOTOR_OPM_SINGLE</a>, 0x00, i + 1); <span class="comment">// Y</span>
<a name="l00121"></a>00121 
<a name="l00122"></a>00122         <a class="code" href="classStepperMotor.html#afe3b8a4013499b95fe9b2f96be809cfe">initSingleMotion</a>(<a class="code" href="config_8h.html#ae58cb2aa8d03cd0343cf58341571128f">MODBUS_SLAVE_ADDR_03</a>, cmd.<a class="code" href="structpar__trajectory__planning_1_1commands__.html#a01e71e9c913c7bc8ec46ac81580f1f08">abs_pos</a>[ 4 + (i * 6) ], cmd.<a class="code" href="structpar__trajectory__planning_1_1commands__.html#a01e71e9c913c7bc8ec46ac81580f1f08">abs_pos</a>[ 5 + (i * 6) ], 
<a name="l00123"></a>00123             <a class="code" href="config_8h.html#a1f042e592f30e895f0e208638bc3c782">MOTOR_ACC_UP</a>, <a class="code" href="config_8h.html#aa7d1af2ddbd04215bc91d5920e3a9d25">MOTOR_ACC_LO</a>, <a class="code" href="config_8h.html#a6d01bebd179d01c2d89014e7193edfb4">MOTOR_DEC_UP</a>, <a class="code" href="config_8h.html#a559e71a5ab59fc849998a0b3bb2b4cc5">MOTOR_DEC_LO</a>,
<a name="l00124"></a>00124             <a class="code" href="config_8h.html#ad8a5d0e0b619e6dc15bd84dd338f805c">MOTOR_OP_SPEED_UP</a>, <a class="code" href="config_8h.html#ab95f25fc2b325b0febe3fcb46d5c6ac1">MOTOR_OP_SPEED_LO</a>, <a class="code" href="config_8h.html#ad4984464598237a917da7f4d07a9fae3">MOTOR_OPM_SINGLE</a>, 0x00, i + 1); <span class="comment">// Z</span>
<a name="l00125"></a>00125     }
<a name="l00126"></a>00126 
<a name="l00127"></a>00127 }
<a name="l00128"></a>00128 
<a name="l00129"></a><a class="code" href="classStepperMotor.html#acfcb85ea01c6274998ae2e2b0a6c17c3">00129</a> <span class="keywordtype">void</span> <a class="code" href="classStepperMotor.html#acfcb85ea01c6274998ae2e2b0a6c17c3" title="Function for configuring PTP motions.">StepperMotor::confPTPMotion</a>(<span class="keyword">const</span> <a class="code" href="structpar__trajectory__planning_1_1commands__.html">par_trajectory_planning::commands</a>&amp; cmd)
<a name="l00130"></a>00130 {
<a name="l00131"></a>00131     <a class="code" href="classStepperMotor.html#af97e94b64dccbdd2bfea15113aea06dc">motions</a> = cmd.<a class="code" href="structpar__trajectory__planning_1_1commands__.html#a453a490acf59796199ef90c0ffe5d0d3">xyz_pos</a>.size() / 3;
<a name="l00132"></a>00132     <a class="code" href="classStepperMotor.html#ab12d157b9b402915289ce35a2c821141">repeat_motions</a> = cmd.<a class="code" href="structpar__trajectory__planning_1_1commands__.html#a08ddc0c8c410cd81ebf6e113ebee31b5">repeat_motions</a>;
<a name="l00133"></a>00133     <span class="keywordflow">if</span> (<a class="code" href="classStepperMotor.html#ab12d157b9b402915289ce35a2c821141">repeat_motions</a> == 0) <a class="code" href="classStepperMotor.html#ab12d157b9b402915289ce35a2c821141">repeat_motions</a> = 1;    
<a name="l00134"></a>00134 
<a name="l00135"></a>00135     uint16_t src[2];
<a name="l00136"></a>00136     modbus_set_slave(<a class="code" href="classStepperMotor.html#a1f622627c7b054721a3313389cb48ad8">ctx</a>, 0);
<a name="l00137"></a>00137         src[1] = cmd.<a class="code" href="structpar__trajectory__planning_1_1commands__.html#a80d1b579fbdf8844808b07c1a5954705">st_speed_lo</a>;
<a name="l00138"></a>00138         src[0] = cmd.<a class="code" href="structpar__trajectory__planning_1_1commands__.html#ae8cf607eb06a57aaa02bddfc90887b00">st_speed_up</a>;
<a name="l00139"></a>00139         <span class="keywordtype">int</span> n = modbus_write_registers(<a class="code" href="classStepperMotor.html#a1f622627c7b054721a3313389cb48ad8">ctx</a>, <a class="code" href="config_8h.html#a0f86d5ed3706f8be146e8a00b3c636a3">REG_MOTOR_ST_SPEED</a>, 2, src);
<a name="l00140"></a>00140         printf(<span class="stringliteral">&quot;errno: %s\n&quot;</span>, modbus_strerror(errno));
<a name="l00141"></a>00141         usleep(<a class="code" href="config_8h.html#a2055321b80999b0cd236f4f934a9dd9c">MODBUS_MAX_PROC_TIME</a>);    
<a name="l00142"></a>00142 
<a name="l00143"></a>00143     <span class="keywordtype">int</span> i;
<a name="l00144"></a>00144     uint16_t seq_pos = 0x01;
<a name="l00145"></a>00145     uint16_t pos_lo[3];
<a name="l00146"></a>00146     uint16_t pos_up[3];
<a name="l00147"></a>00147     <span class="keywordtype">bool</span> invalid_motion[3] = {<span class="keyword">false</span>, <span class="keyword">false</span>, <span class="keyword">false</span>};
<a name="l00148"></a>00148     <span class="keywordflow">for</span>(i=0; i&lt;<a class="code" href="classStepperMotor.html#af97e94b64dccbdd2bfea15113aea06dc">motions</a>; i++)  
<a name="l00149"></a>00149     {
<a name="l00150"></a>00150         std::cout &lt;&lt; <span class="stringliteral">&quot; x : &quot;</span> &lt;&lt; cmd.<a class="code" href="structpar__trajectory__planning_1_1commands__.html#a453a490acf59796199ef90c0ffe5d0d3">xyz_pos</a>[ 0 + (i * 3) ] &lt;&lt; std::endl;
<a name="l00151"></a>00151         std::cout &lt;&lt; <span class="stringliteral">&quot; y : &quot;</span> &lt;&lt; cmd.<a class="code" href="structpar__trajectory__planning_1_1commands__.html#a453a490acf59796199ef90c0ffe5d0d3">xyz_pos</a>[ 1 + (i * 3) ] &lt;&lt; std::endl;
<a name="l00152"></a>00152         std::cout &lt;&lt; <span class="stringliteral">&quot; z : &quot;</span> &lt;&lt; cmd.<a class="code" href="structpar__trajectory__planning_1_1commands__.html#a453a490acf59796199ef90c0ffe5d0d3">xyz_pos</a>[ 2 + (i * 3) ] &lt;&lt; std::endl;
<a name="l00153"></a>00153         pos_lo[<a class="code" href="config_8h.html#a93188324f4ddec8d3bdac778e12231ba">X</a>] = <a class="code" href="classStepperMotor.html#a4ecd40dfd349c51e29eeb326862984cb" title="Function for converting angles in degrees to motor positions.">angleToStep</a>( cmd.<a class="code" href="structpar__trajectory__planning_1_1commands__.html#a453a490acf59796199ef90c0ffe5d0d3">xyz_pos</a>[ 0 + (i * 3) ], invalid_motion[<a class="code" href="config_8h.html#a93188324f4ddec8d3bdac778e12231ba">X</a>] );
<a name="l00154"></a>00154         pos_up[X] = ( pos_lo[X] &amp; 0x8000 ) ? 0xFFFF : 0x00;
<a name="l00155"></a>00155         <span class="comment">//std::cout &lt;&lt; &quot;X: &quot; &lt;&lt; pos_lo[X] &lt;&lt; std::endl;</span>
<a name="l00156"></a>00156         
<a name="l00157"></a>00157         pos_lo[<a class="code" href="config_8h.html#a4245f3c5e8eb90ade09427f989b098ec">Y</a>] = <a class="code" href="classStepperMotor.html#a4ecd40dfd349c51e29eeb326862984cb" title="Function for converting angles in degrees to motor positions.">angleToStep</a>( cmd.<a class="code" href="structpar__trajectory__planning_1_1commands__.html#a453a490acf59796199ef90c0ffe5d0d3">xyz_pos</a>[ 1 + (i * 3) ], invalid_motion[<a class="code" href="config_8h.html#a4245f3c5e8eb90ade09427f989b098ec">Y</a>] );
<a name="l00158"></a>00158         pos_up[Y] = ( pos_lo[Y] &amp; 0x8000 ) ? 0xFFFF : 0x00;
<a name="l00159"></a>00159             <span class="comment">//std::cout &lt;&lt; &quot;Y: &quot; &lt;&lt; pos_lo[Y] &lt;&lt; std::endl;</span>
<a name="l00160"></a>00160         
<a name="l00161"></a>00161         pos_lo[<a class="code" href="config_8h.html#ad6a5acba3306c0d145b10acd89c52126">Z</a>] = <a class="code" href="classStepperMotor.html#a4ecd40dfd349c51e29eeb326862984cb" title="Function for converting angles in degrees to motor positions.">angleToStep</a>( cmd.<a class="code" href="structpar__trajectory__planning_1_1commands__.html#a453a490acf59796199ef90c0ffe5d0d3">xyz_pos</a>[ 2 + (i * 3) ], invalid_motion[<a class="code" href="config_8h.html#ad6a5acba3306c0d145b10acd89c52126">Z</a>] );
<a name="l00162"></a>00162         pos_up[Z] = ( pos_lo[Z] &amp; 0x8000 ) ? 0xFFFF : 0x00;
<a name="l00163"></a>00163             <span class="comment">//std::cout &lt;&lt; &quot;Z: &quot; &lt;&lt; pos_lo[Z] &lt;&lt; std::endl;</span>
<a name="l00164"></a>00164 
<a name="l00165"></a>00165         uint16_t operating_mode = cmd.<a class="code" href="structpar__trajectory__planning_1_1commands__.html#aa079c2d2d83264c4ba9784a9dbc46716">operating_mode</a>[i];
<a name="l00166"></a>00166         <span class="comment">// disable sequential positioning for last entry.</span>
<a name="l00167"></a>00167         seq_pos = (i == (motions - 1)) ? 0x00 : 0x01;
<a name="l00168"></a>00168 
<a name="l00169"></a>00169             <span class="keywordflow">if</span> (invalid_motion[X] ||
<a name="l00170"></a>00170                 invalid_motion[Y] ||
<a name="l00171"></a>00171                 invalid_motion[Z] )
<a name="l00172"></a>00172             {
<a name="l00173"></a>00173                     std::cout &lt;&lt; <span class="stringliteral">&quot;INVALID MOTION; NOT CONFIGURED!&quot;</span> &lt;&lt; std::endl;
<a name="l00174"></a>00174             }
<a name="l00175"></a>00175             <span class="keywordflow">else</span>
<a name="l00176"></a>00176             {
<a name="l00177"></a>00177                     <a class="code" href="classStepperMotor.html#afe3b8a4013499b95fe9b2f96be809cfe">initSingleMotion</a>(<a class="code" href="config_8h.html#ac005ae1021923c6a1e42b0040b1870d6">MODBUS_SLAVE_ADDR_01</a>, pos_up[X], pos_lo[X], 
<a name="l00178"></a>00178                         cmd.<a class="code" href="structpar__trajectory__planning_1_1commands__.html#abbd427e6eaa08f456e90e9c63c1b12b1">acc_lo</a>, cmd.<a class="code" href="structpar__trajectory__planning_1_1commands__.html#a983d5de5106f94488f5ec6c6b8ab003e">acc_up</a>, cmd.<a class="code" href="structpar__trajectory__planning_1_1commands__.html#aabc77ed9e5cbd2711c7fdc401bc9b3a3">dec_lo</a>, cmd.<a class="code" href="structpar__trajectory__planning_1_1commands__.html#a0b2e1138ac67c42d6be2a889979582c1">dec_up</a>, 
<a name="l00179"></a>00179                         cmd.<a class="code" href="structpar__trajectory__planning_1_1commands__.html#a2ca58ae8e81a52b38279d2b74d3566b2">op_speed_lo</a>, cmd.<a class="code" href="structpar__trajectory__planning_1_1commands__.html#a83eff99e3d2d668a934a3c2d4d52674a">op_speed_up</a>, operating_mode, seq_pos, i + 1); <span class="comment">// X</span>
<a name="l00180"></a>00180                     <a class="code" href="classStepperMotor.html#afe3b8a4013499b95fe9b2f96be809cfe">initSingleMotion</a>(<a class="code" href="config_8h.html#a24562b230331cfd7c8661641bd339c95">MODBUS_SLAVE_ADDR_02</a>, pos_up[Y], pos_lo[Y], 
<a name="l00181"></a>00181                         cmd.<a class="code" href="structpar__trajectory__planning_1_1commands__.html#abbd427e6eaa08f456e90e9c63c1b12b1">acc_lo</a>, cmd.<a class="code" href="structpar__trajectory__planning_1_1commands__.html#a983d5de5106f94488f5ec6c6b8ab003e">acc_up</a>, cmd.<a class="code" href="structpar__trajectory__planning_1_1commands__.html#aabc77ed9e5cbd2711c7fdc401bc9b3a3">dec_lo</a>, cmd.<a class="code" href="structpar__trajectory__planning_1_1commands__.html#a0b2e1138ac67c42d6be2a889979582c1">dec_up</a>,
<a name="l00182"></a>00182                         cmd.<a class="code" href="structpar__trajectory__planning_1_1commands__.html#a2ca58ae8e81a52b38279d2b74d3566b2">op_speed_lo</a>, cmd.<a class="code" href="structpar__trajectory__planning_1_1commands__.html#a83eff99e3d2d668a934a3c2d4d52674a">op_speed_up</a>, operating_mode, seq_pos, i + 1); <span class="comment">// Y</span>
<a name="l00183"></a>00183             <a class="code" href="classStepperMotor.html#afe3b8a4013499b95fe9b2f96be809cfe">initSingleMotion</a>(<a class="code" href="config_8h.html#ae58cb2aa8d03cd0343cf58341571128f">MODBUS_SLAVE_ADDR_03</a>, pos_up[Z], pos_lo[Z], 
<a name="l00184"></a>00184                 cmd.<a class="code" href="structpar__trajectory__planning_1_1commands__.html#abbd427e6eaa08f456e90e9c63c1b12b1">acc_lo</a>, cmd.<a class="code" href="structpar__trajectory__planning_1_1commands__.html#a983d5de5106f94488f5ec6c6b8ab003e">acc_up</a>, cmd.<a class="code" href="structpar__trajectory__planning_1_1commands__.html#aabc77ed9e5cbd2711c7fdc401bc9b3a3">dec_lo</a>, cmd.<a class="code" href="structpar__trajectory__planning_1_1commands__.html#a0b2e1138ac67c42d6be2a889979582c1">dec_up</a>, 
<a name="l00185"></a>00185                 cmd.<a class="code" href="structpar__trajectory__planning_1_1commands__.html#a2ca58ae8e81a52b38279d2b74d3566b2">op_speed_lo</a>, cmd.<a class="code" href="structpar__trajectory__planning_1_1commands__.html#a83eff99e3d2d668a934a3c2d4d52674a">op_speed_up</a>, operating_mode, seq_pos, i + 1); <span class="comment">// Z </span>
<a name="l00186"></a>00186             }
<a name="l00187"></a>00187     }
<a name="l00188"></a>00188 }
<a name="l00189"></a>00189 
<a name="l00190"></a><a class="code" href="classStepperMotor.html#ab34933b2081d2f367700bfc13157a1b2">00190</a> <span class="keywordtype">void</span> <a class="code" href="classStepperMotor.html#ab34933b2081d2f367700bfc13157a1b2" title="Function for exiting.">StepperMotor::exit</a>()
<a name="l00191"></a>00191 {
<a name="l00192"></a>00192     std::cout &lt;&lt; <span class="stringliteral">&quot;StepperMotor::exit()&quot;</span> &lt;&lt; std::endl;
<a name="l00193"></a>00193     <span class="keywordflow">if</span> (<a class="code" href="classStepperMotor.html#a1f622627c7b054721a3313389cb48ad8">ctx</a> != NULL)
<a name="l00194"></a>00194     {
<a name="l00195"></a>00195             <span class="comment">//modbus_close(ctx);</span>
<a name="l00196"></a>00196             <span class="comment">//modbus_free(ctx);    </span>
<a name="l00197"></a>00197     }
<a name="l00198"></a>00198 }
<a name="l00199"></a>00199 
<a name="l00200"></a><a class="code" href="classStepperMotor.html#afe3b8a4013499b95fe9b2f96be809cfe">00200</a> <span class="keywordtype">void</span> <a class="code" href="classStepperMotor.html#afe3b8a4013499b95fe9b2f96be809cfe">StepperMotor::initSingleMotion</a>(<span class="keywordtype">int</span> slave, uint16_t pos_lo, uint16_t pos_up, 
<a name="l00201"></a>00201     uint16_t acc_lo, uint16_t acc_up, uint16_t dec_lo, uint16_t dec_up, 
<a name="l00202"></a>00202     uint16_t speed_lo, uint16_t speed_up, uint16_t operating_mode, uint16_t seq_pos,
<a name="l00203"></a>00203     <span class="keywordtype">int</span> off)
<a name="l00204"></a>00204 {
<a name="l00205"></a>00205         uint16_t src[2];
<a name="l00206"></a>00206         <span class="keywordtype">int</span> n;
<a name="l00207"></a>00207         <span class="comment">// position is address 0x0402 and 0x0403 and specify position pos_up / pos_lo</span>
<a name="l00208"></a>00208         modbus_set_slave(<a class="code" href="classStepperMotor.html#a1f622627c7b054721a3313389cb48ad8">ctx</a>, slave);
<a name="l00209"></a>00209 
<a name="l00210"></a>00210         src[1] = pos_up;
<a name="l00211"></a>00211         src[0] = pos_lo;
<a name="l00212"></a>00212         n = modbus_write_registers(<a class="code" href="classStepperMotor.html#a1f622627c7b054721a3313389cb48ad8">ctx</a>, <a class="code" href="config_8h.html#a7c2af378114c1e7f984da9db6a36194a">REG_MOTOR_POS</a> + (off * 2), 2, src);
<a name="l00213"></a>00213         <span class="comment">//printf(&quot;errno: %s\n&quot;, modbus_strerror(errno));</span>
<a name="l00214"></a>00214         usleep(<a class="code" href="config_8h.html#a2055321b80999b0cd236f4f934a9dd9c">MODBUS_MAX_PROC_TIME</a>);
<a name="l00215"></a>00215         
<a name="l00216"></a>00216         <span class="comment">// set operating speed</span>
<a name="l00217"></a>00217         src[1] = speed_up;
<a name="l00218"></a>00218         src[0] = speed_lo;
<a name="l00219"></a>00219         std::cout &lt;&lt; <span class="stringliteral">&quot;speed_up: &quot;</span> &lt;&lt; speed_up &lt;&lt; std::endl;
<a name="l00220"></a>00220     std::cout &lt;&lt; <span class="stringliteral">&quot;speed_lo: &quot;</span> &lt;&lt; speed_lo &lt;&lt; std::endl;
<a name="l00221"></a>00221         n = modbus_write_registers(<a class="code" href="classStepperMotor.html#a1f622627c7b054721a3313389cb48ad8">ctx</a>, <a class="code" href="config_8h.html#a12921b667d601782d605651f6db2937b">REG_MOTOR_OP_SPEED</a> + (off * 2), 2, src);
<a name="l00222"></a>00222         <span class="comment">//printf(&quot;errno: %s\n&quot;, modbus_strerror(errno));</span>
<a name="l00223"></a>00223         usleep(<a class="code" href="config_8h.html#a2055321b80999b0cd236f4f934a9dd9c">MODBUS_MAX_PROC_TIME</a>);
<a name="l00224"></a>00224         
<a name="l00225"></a>00225         <span class="comment">// set acceleration</span>
<a name="l00226"></a>00226         src[1] = acc_up;
<a name="l00227"></a>00227         src[0] = acc_lo;
<a name="l00228"></a>00228         std::cout &lt;&lt; <span class="stringliteral">&quot;acc_up: &quot;</span> &lt;&lt; acc_up &lt;&lt; std::endl;
<a name="l00229"></a>00229     std::cout &lt;&lt; <span class="stringliteral">&quot;acc_lo: &quot;</span> &lt;&lt; acc_lo &lt;&lt; std::endl;
<a name="l00230"></a>00230         n = modbus_write_registers(<a class="code" href="classStepperMotor.html#a1f622627c7b054721a3313389cb48ad8">ctx</a>, <a class="code" href="config_8h.html#a6cec9b600249544d4fdafffe1e73e058">REG_MOTOR_ACC</a> + (off * 2), 2, src);
<a name="l00231"></a>00231         <span class="comment">//printf(&quot;errno: %s\n&quot;, modbus_strerror(errno));</span>
<a name="l00232"></a>00232         usleep(<a class="code" href="config_8h.html#a2055321b80999b0cd236f4f934a9dd9c">MODBUS_MAX_PROC_TIME</a>);   
<a name="l00233"></a>00233         
<a name="l00234"></a>00234         <span class="comment">// set deceleration</span>
<a name="l00235"></a>00235         src[1] = dec_up;
<a name="l00236"></a>00236         src[0] = dec_lo;
<a name="l00237"></a>00237         std::cout &lt;&lt; <span class="stringliteral">&quot;dec_up: &quot;</span> &lt;&lt; dec_up &lt;&lt; std::endl;
<a name="l00238"></a>00238     std::cout &lt;&lt; <span class="stringliteral">&quot;dec_lo: &quot;</span> &lt;&lt; dec_lo &lt;&lt; std::endl;     
<a name="l00239"></a>00239         n = modbus_write_registers(<a class="code" href="classStepperMotor.html#a1f622627c7b054721a3313389cb48ad8">ctx</a>, <a class="code" href="config_8h.html#ae43e76c10d1dcc8e88ddba7588d5a702">REG_MOTOR_DEC</a> + (off * 2), 2, src);
<a name="l00240"></a>00240         <span class="comment">//printf(&quot;errno: %s\n&quot;, modbus_strerror(errno));</span>
<a name="l00241"></a>00241         usleep(<a class="code" href="config_8h.html#a2055321b80999b0cd236f4f934a9dd9c">MODBUS_MAX_PROC_TIME</a>);           
<a name="l00242"></a>00242 
<a name="l00243"></a>00243         <span class="comment">// set to absolute positioning</span>
<a name="l00244"></a>00244         n = modbus_write_register(<a class="code" href="classStepperMotor.html#a1f622627c7b054721a3313389cb48ad8">ctx</a>, <a class="code" href="config_8h.html#aff7913b34a2ee2dbde76af813a94f47d">REG_MOTOR_MPOS</a> + (off * 1), 0x01);       
<a name="l00245"></a>00245         <span class="comment">//printf(&quot;errno: %s\n&quot;, modbus_strerror(errno));</span>
<a name="l00246"></a>00246         usleep(<a class="code" href="config_8h.html#a2055321b80999b0cd236f4f934a9dd9c">MODBUS_MAX_PROC_TIME</a>);
<a name="l00247"></a>00247         
<a name="l00248"></a>00248         <span class="comment">// set operating mode</span>
<a name="l00249"></a>00249         n = modbus_write_register(<a class="code" href="classStepperMotor.html#a1f622627c7b054721a3313389cb48ad8">ctx</a>, <a class="code" href="config_8h.html#a272b0d9072163793a2d911555d2c1a96">REG_MOTOR_OPM</a> + (off * 1), operating_mode);
<a name="l00250"></a>00250         usleep(<a class="code" href="config_8h.html#a2055321b80999b0cd236f4f934a9dd9c">MODBUS_MAX_PROC_TIME</a>);
<a name="l00251"></a>00251         
<a name="l00252"></a>00252         <span class="comment">// set to sequential positioning</span>
<a name="l00253"></a>00253         n = modbus_write_register(<a class="code" href="classStepperMotor.html#a1f622627c7b054721a3313389cb48ad8">ctx</a>, <a class="code" href="config_8h.html#a9377caf31b895a0abea35b5f37622311">REG_MOTOR_SQPS</a> + (off * 1), seq_pos);
<a name="l00254"></a>00254         usleep(<a class="code" href="config_8h.html#a2055321b80999b0cd236f4f934a9dd9c">MODBUS_MAX_PROC_TIME</a>);
<a name="l00255"></a>00255 
<a name="l00256"></a>00256     <span class="comment">/*</span>
<a name="l00257"></a>00257 <span class="comment">        if (slave == 1)</span>
<a name="l00258"></a>00258 <span class="comment">        {</span>
<a name="l00259"></a>00259 <span class="comment">                src[0] = 0;</span>
<a name="l00260"></a>00260 <span class="comment">                src[1] = 0;</span>
<a name="l00261"></a>00261 <span class="comment">                n = modbus_read_registers(ctx, 0x0019, 1, src);</span>
<a name="l00262"></a>00262 <span class="comment">                usleep(MODBUS_MAX_PROC_TIME);</span>
<a name="l00263"></a>00263 <span class="comment">                printf(&quot;ACCELERATION RATE FOR SELECT DATA NUMBER (LOWER): %x\n&quot;, src[0]);</span>
<a name="l00264"></a>00264 <span class="comment">        }</span>
<a name="l00265"></a>00265 <span class="comment">        */</span>
<a name="l00266"></a>00266 }
<a name="l00267"></a>00267 
<a name="l00268"></a><a class="code" href="classStepperMotor.html#a4ecd40dfd349c51e29eeb326862984cb">00268</a> uint16_t <a class="code" href="classStepperMotor.html#a4ecd40dfd349c51e29eeb326862984cb" title="Function for converting angles in degrees to motor positions.">StepperMotor::angleToStep</a>(<span class="keywordtype">double</span> x, <span class="keywordtype">bool</span>&amp; invalid_motion)
<a name="l00269"></a>00269 {
<a name="l00270"></a>00270         <span class="comment">//std::cout &lt;&lt; &quot;ANGLE: &quot; &lt;&lt; x &lt;&lt; std::endl;</span>
<a name="l00271"></a>00271         <span class="keywordflow">if</span> (x != x)
<a name="l00272"></a>00272         {
<a name="l00273"></a>00273             invalid_motion = <span class="keyword">true</span>;
<a name="l00274"></a>00274         }
<a name="l00275"></a>00275         <span class="keywordflow">else</span>
<a name="l00276"></a>00276         {
<a name="l00277"></a>00277                 invalid_motion = <span class="keyword">false</span>;
<a name="l00278"></a>00278         }
<a name="l00279"></a>00279         <span class="keywordflow">return</span> <span class="keyword">static_cast&lt;</span>uint16_t<span class="keyword">&gt;</span>(round((90.0 + x) / <a class="code" href="config_8h.html#a3f5c85ceab799325b1e6b85345f473f9">MOTOR_STEP_ANGLE</a>));
<a name="l00280"></a>00280 }
<a name="l00281"></a>00281 
<a name="l00282"></a><a class="code" href="classStepperMotor.html#a9f9b3d4bf835d5b157c2b9a8f015c87c">00282</a> <span class="keywordtype">void</span> <a class="code" href="classStepperMotor.html#a9f9b3d4bf835d5b157c2b9a8f015c87c" title="Function for developing purposes.">StepperMotor::test</a>()
<a name="l00283"></a>00283 {
<a name="l00284"></a>00284 }
</pre></div></div>
</div>
</td></tr></table>

<hr size="1"><div style="align: right;">
<a href="http://ros.org/wiki/par_trajectory_planning">par_trajectory_planning</a><br />
Author(s): Wouter van Teijlingen</br />
<small>autogenerated on Wed Jun 8 2011 17:11:45</small>
</div>
</body>
</html>
